package lego.jbv;

import lejos.nxt.*;
import lejos.nxt.comm.*;
import lejos.robotics.Color;
import lejos.util.Delay;

import java.io.*;


public class BTController extends Thread {
    private static String connected = "Connected";
    private static String waiting = "Waiting...";
    private static String closing = "Closing...";

    private static BTConnection btc;
    private static DataInputStream dis;
    
    private int leftPower = -100;
    private int rightPower = -100;
    
    private long startTime = 0;
    private long finishTime = 0;
    
    public BTController(){
    	btc = null;
    	setDaemon(true);
    }
    
    public void run() 
    {
    	LCD.drawString(waiting,0,0);

        btc = Bluetooth.waitForConnection();
        
        LCD.clear();
        LCD.drawString(connected,0,0);	

        dis = btc.openDataInputStream();
              
        read();
    }
    
    private void read()
    {
    	while(btc.available() <= 0)
    	{
    		Delay.msDelay(10);
    	}
    	
    	startTime = System.currentTimeMillis();
    	
    	while(true)
        {
    		try
            {
            	if(btc.available() > 0)
            	{
            		leftPower = dis.readInt();
            		rightPower = dis.readInt();
            		
            		LCD.drawInt(leftPower, 4, 0, 3);
            		LCD.drawInt(rightPower, 4, 6, 3);
            		
            		if(leftPower == 404)
            		{
        				ColorSensor cs = new ColorSensor(SensorPort.S1);
        				cs.setFloodlight(Color.WHITE);
            			
        				Car.drive(-75, -75);
        				LCD.clear();
        				
        				while(true)
            			{
        					LCD.drawString(Integer.toString((cs.getColor()).getColor()) , 0, 4);
            				if((cs.getColor()).getColor() == 1)
            				{
            					finishTime = System.currentTimeMillis();
            					Car.stop();
            					LCD.drawString(Long.toString(finishTime-startTime), 0, 2);
            					Delay.msDelay(1000);
            					break;
            				}	
            			}
            		}
            		
            		else if(leftPower == 200)
            		{
            			leftPower = -75;
            			rightPower = 75;
            		}
            		
            		else if(leftPower == 300)
            		{
            			/*leftPower = 0;
            			rightPower = 0;*/
            			
            			/*while(btc.available() <= 0)
            			{
            				Delay.msDelay(10);
            			}*/
            		}
            	}
            }
    		
            catch (Exception e)
            {
            	LCD.clear();
            	LCD.drawString("Read fail", 0, 0);
            }
    		
    		Delay.msDelay(10);
        }
    }
		   
    public void stop() 
    {
        LCD.clear();
        LCD.drawString(closing,0,0);
    	try 
    	{
    	    dis.close();
            Thread.sleep(100); // wait for data to drain
            btc.close();    	
    	}
        catch (Exception e)
        {
        	LCD.drawString("Stop fail", 0, 0);
        }
        try {Thread.sleep(1000);}catch (Exception e){}
        System.exit(0);
    }
    
    public synchronized int getLeftPower()
    {
    	return leftPower;
    }
    public synchronized long getStartTime()
    {
    	return startTime;
    }
    
    public synchronized int getRightPower()
    {
    	return rightPower;
    }
    
    public synchronized void setPower(int left, int right)
    {
    	leftPower = left;
    	rightPower = right;
    }
    
    public synchronized void setStartTime(long _startTime)
    {
    	startTime = _startTime;
    }
}